#include "SkyPhoto.h"
#include "Camera.h"
#include "../sky/ClearSky.h"
#include "../sky/Sun.h"
#include "../dialog/SkyFeatureDialog.h"
#include <cmath>
#include <fstream>
using namespace std;



CSkyPhoto::CSkyPhoto(char* filename, CCamera* pCamera)
	: CPhoto(pCamera)
{
	m_pSky = NULL;
	int i = 0;
	while (filename[i])
	{
		m_fileName[i] = filename[i];
		i++;
	}
	m_fileName[i] = '\0';
	m_samples.clear();
	LoadSamplesFromFile();
	
}

CSkyPhoto::CSkyPhoto(char* filename, CString imagePath)
	: CPhoto(imagePath)
{
	m_pSky = NULL;
	int i = 0;
	while (filename[i])
	{
		m_fileName[i] = filename[i];
		i++;
	}
	m_fileName[i] = '\0';
	m_samples.clear();
	LoadSamplesFromFile();
}

void CSkyPhoto::SetSky(CClearSky* pSky)
{
	m_pSky = pSky;
}

CSkyPhoto::CSkyPhoto(const CSkyPhoto& photo)
	: CPhoto(photo)
{
	m_solarPosition = photo.m_solarPosition;
	m_pSky = photo.m_pSky;
	m_samples.clear();
	int n = photo.m_samples.size();
	for (int i = 0; i < n; i++)
		m_samples.push_back(photo.m_samples[i]);

	int i = 0;
	while (photo.m_fileName[i])
	{
		m_fileName[i] = photo.m_fileName[i];
		i++;
	}
	COLOR_INDEX color = RED;
	for (color = RED; color <= BLUE; color = (COLOR_INDEX)((int)color + 1))
		m_paras[color] = photo.m_paras[color];
	m_fileName[i] = '\0';
}

void CSkyPhoto::CalcPixelSolarPosition()
{
	int width = m_image->width;
	int height = m_image->height;
	CPixel pixel;
	CColor color;
	float length = 0.0f;
	float totLength = 0.0f;
	float x = 0.0f;
	float y = 0.0f;
	for (int i = 0; i < height; i++)
	{
		for (int j = 0; j < width; j++)
		{
			pixel.m_x = j;
			pixel.m_y = i;
			Pixel2Color(pixel, color);
			if (color.m_r >= 250 && color.m_g >= 250 && color.m_b >= 250)
			{
				length = sqrt((float)(color.m_r * color.m_r + color.m_g * color.m_g + color.m_b * color.m_b));
				totLength += length;
				x += length * j;
				y += length * i;
			}
		}
	}
	if (totLength > 0.0f)
	{
		m_isSunny = true;
		m_solarPosition.m_x = (int)(x / totLength);
		m_solarPosition.m_y = (int)(y / totLength);
	}
	else
	{
		//	when it is not a sunny day, we assume the camera points at a specific sky element that is assigned
		//	by the user, and m_soloarPosition is set to be the mid of the photo
		m_isSunny = false;
		m_solarPosition.m_x = width / 2;
		m_solarPosition.m_y = height / 2;
	}
}

void CSkyPhoto::CalcExtrinsicParas()
{
	CPixel pixel = m_solarPosition;
	CPoint3D p3D;
	if (m_isSunny)
		p3D = m_pSky->GetSun()->GetGloSolarPosition();
	else
	{
		CSkyFeatureDialog skyFeatureDlg;
		if (skyFeatureDlg.DoModal() == IDOK)
		{
			p3D.SetPoint(skyFeatureDlg.GetFeaturePointX(), 
				skyFeatureDlg.GetFeaturePointY(), 
				skyFeatureDlg.GetFeaturePointZ());
		}
	}
	//	we assume it is a image for modeling sky appearance
	//	pixel is the position of the sun in the image
	//	point is the position of the sun in the global world
	//	only direction is necessary

	//	camera coord:
	//	x:	horizontal(left-right)
	//	y:	vertical(up-down)
	//	z:	towards the front

	//	sky	coord:
	//	x:	horizontal(left-right, West-East)
	//	y:	towards the front(South-North)
	//	z:	vertical(down-up)
	//	sun elevation:	xoy
	//	sun azimuth:	starting from y: y->x

	//	assumption:	x-axis in camera coord is ALWAYS parallel
	//	to the xoy surface in sky coord

	CV_MAT_ELEM(*m_translationVector, float, 0, 0) = 0.0f;
	CV_MAT_ELEM(*m_translationVector, float, 0, 1) = 0.0f;
	CV_MAT_ELEM(*m_translationVector, float, 0, 2) = 0.0f;
	CPoint3D p3DInCamera;
	m_pCamera->Pixel2CameraPoint3D(pixel, p3DInCamera);
	p3D.Normalize();
	p3DInCamera.Normalize();

	CvMat* rotationMatrix = cvCreateMat(3, 3, CV_32FC1);

	//	each ROW in the matrix indicates the representation of the
	//	basis in the camera coords, consider the following:
	//	row vector [glo] = row vector [cam] * rotation Matrix(3 * 3)

	float theta1 = atan2f(p3D.m_y, p3D.m_x);
	float theta2 = asin(sqrt(((1 - p3DInCamera.m_x * p3DInCamera.m_x) - p3D.m_z * p3D.m_z) /
		(1 - p3D.m_z * p3D.m_z)));
	if (p3DInCamera.m_x < 0)
	{
		theta2 = (float)M_PI - theta2;
	}

	//	here we have TWO possible solutions:
	//	the angle between the two x-axis could be either theta1 + theta2 or theta1 - theta2

	//	for the case theta1 + theta2
	CPoint3D iBasis, jBasis, kBasis;
	iBasis.m_z = 0.0f;
	iBasis.m_x = cos(theta1 + theta2);
	iBasis.m_y = sin(theta1 + theta2);		
	if (!CalcCoords(p3D, p3DInCamera, iBasis, jBasis, kBasis))
	{
		//	for the case theta1 - theta2
		iBasis.m_z = 0.0f;
		iBasis.m_x = cos(theta1 - theta2);
		iBasis.m_y = sin(theta1 - theta2);
		CalcCoords(p3D, p3DInCamera, iBasis, jBasis, kBasis);
	}

	CV_MAT_ELEM(*rotationMatrix, float, 0, 0) = iBasis.m_x;
	CV_MAT_ELEM(*rotationMatrix, float, 0, 1) = iBasis.m_y;
	CV_MAT_ELEM(*rotationMatrix, float, 0, 2) = iBasis.m_z;

	CV_MAT_ELEM(*rotationMatrix, float, 1, 0) = jBasis.m_x;
	CV_MAT_ELEM(*rotationMatrix, float, 1, 1) = jBasis.m_y;
	CV_MAT_ELEM(*rotationMatrix, float, 1, 2) = jBasis.m_z;

	CV_MAT_ELEM(*rotationMatrix, float, 2, 0) = kBasis.m_x;
	CV_MAT_ELEM(*rotationMatrix, float, 2, 1) = kBasis.m_y;
	CV_MAT_ELEM(*rotationMatrix, float, 2, 2) = kBasis.m_z;

	cvRodrigues2(rotationMatrix, m_rotationVector, 0);
	cvReleaseMat(&rotationMatrix);

	CPoint3D p;
	p.SetPoint(0.0f, 0.0f, 0.0f);
	CameraPoint3D2GlobalPoint3D(p, m_cameraPosition);
}

void CSkyPhoto::AddSample(CPixel p, CColor c)
{
	CSample sample = {p, c};
	m_samples.push_back(sample);
}

void CSkyPhoto::SaveSamplesIntoFile()
{
	ofstream fout(m_fileName);
	int length = m_samples.size();
	fout << length << endl;
	for (int i = 0; i < length; i++)
	{
		fout << m_samples[i].m_pixel.m_x << ' ' << m_samples[i].m_pixel.m_y << ' '
			<< m_samples[i].m_color.m_r << ' ' << m_samples[i].m_color.m_g << ' '
			<< m_samples[i].m_color.m_b << endl;
	}
	for (int i = 0; i < 3; i++)
	{
		fout << m_paras[i].m_a << ' ' 
			<< m_paras[i].m_b << ' ' 
			<< m_paras[i].m_c << ' '
			<< m_paras[i].m_d << ' '
			<< m_paras[i].m_e << ' '
			<< m_paras[i].m_Lp << endl;
	}
		
	fout.close();
}

bool CSkyPhoto::LoadSamplesFromFile()
{
	ifstream fin(m_fileName);
	if (!fin.is_open())
		return false;
	m_samples.clear();
	int length;
	fin >> length;
	CSample sample;
	for (int i = 0; i < length; i++)
	{
		fin >> sample.m_pixel.m_x >> sample.m_pixel.m_y >> sample.m_color.m_r
			>> sample.m_color.m_g >> sample.m_color.m_b;
		m_samples.push_back(sample);
	}
	for (int i = 0; i < 3; i++)
	{	
		fin >> m_paras[i].m_a 
			>> m_paras[i].m_b
			>> m_paras[i].m_c
			>> m_paras[i].m_d
			>> m_paras[i].m_e
			>> m_paras[i].m_Lp;
	}	
	fin.close();
	return true;
}

void CSkyPhoto::SkyModelFitting()
{
	//	here we do the optimization to fill in the m_paras
	//	including a, b, c, d, e and Lp
	//	what we have known are the samples and solar position

	

	vector<float> thetas;
	vector<float> gammas;
	vector<CColor> colors;

	int length = m_samples.size();
	CPoint3D point;
	CPoint3D solarPoint;
	//ofstream fout("points.data");
	solarPoint = m_pSky->GetSun()->GetGloSolarPosition();
	solarPoint.Normalize();
	//fout << solarPoint.m_x << ' ' << solarPoint.m_y << ' ' << solarPoint.m_z << endl;
	//ofstream fcolor("color.data");
	for (int i = 0; i < length; i++)
	{
		Pixel2GlobalPoint3D(m_samples[i].m_pixel, point);
		if (point.m_z < 0)
			continue;
		point.Normalize();
		if (abs(point.m_z) < 1e-3)
			point.m_z = 1e-3;
		
		//fout << point.m_x << ' ' << point.m_y << ' ' << point.m_z << endl;
		thetas.push_back(acosf(point.m_z));
		gammas.push_back(acosf(point.m_x * solarPoint.m_x 
			+ point.m_y * solarPoint.m_y
			+ point.m_z * solarPoint.m_z));
		colors.push_back(m_samples[i].m_color);
		//fcolor << m_samples[i].m_color.m_r << ' ' << m_samples[i].m_color.m_g << ' ' << m_samples[i].m_color.m_b << endl;
	}
	//fout.close();
	//fcolor.close();
	length = thetas.size();
	//	initialize paras for R, G, B
	float energy[3], preEnergy[3], energyTerm;
	float delta[5][3];
	float step = 1e-5;

	float FiF[3], FF[3];	
	float pf;

	int iteration = 0;
	COLOR_INDEX cindex;
	PARA_INDEX pindex;
	for (cindex = RED; cindex <= BLUE; cindex = (COLOR_INDEX)(cindex + 1))
	{
		m_paras[cindex].SetParameters(-1.0f, -0.32f, 10.0f, -3.0f, 0.45f, 1.0f);
		preEnergy[cindex] = 0.0f;
	}

	while(true)
	{
		for (cindex = RED; cindex <= BLUE; cindex = (COLOR_INDEX)(cindex + 1))
		{
			FiF[cindex] = FF[cindex] = 0.0f;
			//	Lp optimization
			for (int i = 0; i < length; i++)
			{
				pf = PerezFunction(m_paras[cindex], thetas[i], gammas[i]);
				FiF[cindex] += pf * colors[i][cindex];
				FF[cindex] += pf * pf;
			}

			m_paras[cindex].m_Lp = FiF[cindex] / FF[cindex];
			//	a, b, c, d, e optimization
			for (int i = 0; i < 5; i++)
					delta[i][cindex] = 0.0f;
			energy[cindex] = 0.0f;

			for (int i = 0; i < length; i++)
			{
				energyTerm = m_paras[cindex].m_Lp * PerezFunction(m_paras[cindex], thetas[i], gammas[i]) - colors[i][cindex];
				energy[cindex] += energyTerm * energyTerm;
				energyTerm *= 2 * m_paras[cindex].m_Lp;
				for (pindex = A; pindex <= E; pindex = (PARA_INDEX)(pindex + 1))
				{
					delta[pindex][cindex] += energyTerm * dPdparas(m_paras[cindex], thetas[i], gammas[i], pindex);
				}
			}
		}
		if ((abs(preEnergy[RED] - energy[RED]) < 1e-3
			&& abs(preEnergy[GREEN] - energy[GREEN]) < 1e-3
			&& abs(preEnergy[BLUE] - energy[BLUE]) < 1e-3)
			|| iteration > 20000)
			break;

		preEnergy[RED] = energy[RED];
		preEnergy[GREEN] = energy[GREEN];
		preEnergy[BLUE] = energy[BLUE];

		for (int i = 0; i < 3; i++)
		{
			m_paras[i].m_a -= step * delta[A][i];
			m_paras[i].m_b -= step * delta[B][i];
			m_paras[i].m_c -= step * delta[C][i];
			m_paras[i].m_d -= step * delta[D][i];
			m_paras[i].m_e -= step * delta[E][i];
		}
		iteration++;
	}
}

float CSkyPhoto::PerezFunction(CClearSkyParameter para, float theta, float gamma)
{
	float cosGamma = cos(gamma);
	return (1 + para.m_a * exp(para.m_b / cos(theta))) 
		* (1 + para.m_c * exp(para.m_d * gamma) + para.m_e * cosGamma * cosGamma);
}

float CSkyPhoto::dPda(CClearSkyParameter para, float theta, float gamma)
{
	float cosGamma = cos(gamma);
	return exp(para.m_b / cos(theta)) 
		* (1 + para.m_c * exp(para.m_d * gamma) + para.m_e * cosGamma * cosGamma);
}

float CSkyPhoto::dPdb(CClearSkyParameter para, float theta, float gamma)
{
	float cosGamma = cos(gamma);
	float cosTheta = cos(theta);
	return (para.m_a * exp(para.m_b / cosTheta) / cosTheta)
		* (1 + para.m_c * exp(para.m_d * gamma) + para.m_e * cosGamma * cosGamma);
}

float CSkyPhoto::dPdc(CClearSkyParameter para, float theta, float gamma)
{
	return (1 + para.m_a * exp(para.m_b / cos(theta))) * exp(para.m_d * gamma);
}

float CSkyPhoto::dPdd(CClearSkyParameter para, float theta, float gamma)
{
	return (1 + para.m_a * exp(para.m_b / cos(theta))) * (para.m_c * exp(para.m_d * gamma) * gamma);
}

float CSkyPhoto::dPde(CClearSkyParameter para, float theta, float gamma)
{
	float cosGamma = cos(gamma);
	return (1 + para.m_a * exp(para.m_b / cos(theta))) * cosGamma * cosGamma;
}

void CSkyPhoto::Glo3DPoint2SkyModelColor(float x, float y, float z, int &r, int &g, int &b)
{
	CPoint3D point;
	CPoint3D solarPoint;
	solarPoint = m_pSky->GetSun()->GetGloSolarPosition();
	solarPoint.Normalize();

	point.SetPoint(x, y, z);
	point.Normalize();
	if (abs(point.m_z) < 1e-3)
		point.m_z = 1e-3;
	float theta, gamma;
	theta = acosf(point.m_z);
	gamma = acosf(point.m_x * solarPoint.m_x 
		+ point.m_y * solarPoint.m_y
		+ point.m_z * solarPoint.m_z);
	r = m_paras[RED].m_Lp * PerezFunction(m_paras[RED], theta, gamma);
	g = m_paras[GREEN].m_Lp * PerezFunction(m_paras[GREEN], theta, gamma);
	b = m_paras[BLUE].m_Lp * PerezFunction(m_paras[BLUE], theta, gamma);
	r = (r > 255) ? 255 : r;
	g = (g > 255) ? 255 : g;
	b = (b > 255) ? 255 : b;
}

void CSkyPhoto::Pixel2SkyModelColor(int x, int y, int &r, int &g, int &b)
{
	CPoint3D point;
	CPoint3D solarPoint;
	solarPoint = m_pSky->GetSun()->GetGloSolarPosition();
	solarPoint.Normalize();

	CPixel pixel;
	pixel.m_x = x; pixel.m_y = y;
	Pixel2GlobalPoint3D(pixel, point);
	point.Normalize();
	if (abs(point.m_z) < 1e-3)
		point.m_z = 1e-3;
	float theta, gamma;
	theta = acosf(point.m_z);
	gamma = acosf(point.m_x * solarPoint.m_x 
			+ point.m_y * solarPoint.m_y
			+ point.m_z * solarPoint.m_z);
	r = m_paras[RED].m_Lp * PerezFunction(m_paras[RED], theta, gamma);
	g = m_paras[GREEN].m_Lp * PerezFunction(m_paras[GREEN], theta, gamma);
	b = m_paras[BLUE].m_Lp * PerezFunction(m_paras[BLUE], theta, gamma);
	r = (r > 255) ? 255 : r;
	g = (g > 255) ? 255 : g;
	b = (b > 255) ? 255 : b;
}

float CSkyPhoto::dPdparas(CClearSkyParameter para, float theta, float gamma, PARA_INDEX pindex)
{
	switch (pindex)
	{
	case A:
		return dPda(para, theta, gamma);
	case B:
		return dPdb(para, theta, gamma);
	case C:
		return dPdc(para, theta, gamma);
	case D:
		return dPdd(para, theta, gamma);
	case E:
		return dPde(para, theta, gamma);
	default:
		return 0.0f;
	}
}

void CSkyPhoto::SaveSkyPhoto()
{
	char filename[256];
	CString name(m_fileName);
	int start = name.ReverseFind('/');
	int end = name.ReverseFind('.');
	name = name.Mid(start + 1, end - start - 1);
	CString header("../sky/");
	CString ext("_sky.bmp");
	CString totalName = header + name + ext;
	Str2Char(totalName, filename);
	cvSaveImage(filename, m_image);
}